"""
author: Xie Dongyu
date: 2023-10-14
desc: 机器视觉光点检测算法
"""


import numpy as np

# import matplotlib.pyplot as plt
import cv2 as cv
import scipy.signal as ss


# 生成高斯核
def gauss_seperate_kernel(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    # 高斯核的可分解性
    x_m = np.arange(m) - m // 2  # //在Python中表示整数除法，返回不大于结果的一个最大的整数，意思就是除法结果向下取整
    y_m = (
        1
        / (np.sqrt(2 * np.pi) * sigma)
        * np.exp(-(x_m**2) / (2 * sigma**2)).reshape(-1, 1)
    )

    x_n = np.arange(n) - n // 2
    y_n = (
        1
        / (np.sqrt(2 * np.pi) * sigma)
        * np.exp(-(x_n**2) / (2 * sigma**2)).reshape(-1, 1)
    )
    kernel = y_m * y_n.T  # 没归一化
    return kernel


# 生成二维高斯x方向一阶导，即gx模板
def gauss_gx(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [2 1  0  -1  -2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                -i
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 4))
            )
            kernel[kj, ki] = value
    return kernel


# 生成二维高斯y方向一阶导，即gy模板
def gauss_gy(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [-2 -1  0  1  2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                -j
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 4))
            )
            kernel[kj, ki] = value
    return kernel


# 生成二维高斯x方向二阶导，即gxx模板
def gauss_gxx(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [-2 -1  0  1  2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                (i**2 - sigma**2)
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 6))
            )
            kernel[kj, ki] = value
    return kernel


# 生成二维高斯y方向二阶导，即gyy模板
def gauss_gyy(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [-2 -1  0  1  2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                (j**2 - sigma**2)
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 6))
            )
            kernel[kj, ki] = value
    return kernel


# 生成二维高斯xy方向二阶导，即gxy模板
def gauss_gxy(kernel_size, sigma=1):
    if sigma == 0:
        sigma = ((kernel_size - 1) * 0.5 - 1) * 0.3 + 0.8
    m = kernel_size[0]
    n = kernel_size[1]
    kernel = np.zeros((m, n))
    x_m = np.arange(m - 1, -1, -1) - m // 2  # [-2 -1  0  1  2]
    y_n = np.arange(n - 1, -1, -1) - n // 2
    for i in x_m:
        for j in y_n:
            ki = m // 2 - i
            kj = n // 2 - j
            value = (
                i
                * j
                * np.exp(-(i * i + j * j) / (2 * sigma * sigma))
                / (2 * np.pi * np.power(sigma, 6))
            )
            kernel[kj, ki] = value
    return kernel


def zero_pad(image, pad):
    """
    对图像进行零填充
    :param image: 原始图像，numpy数组，shape为(height, width)
    :param pad: 填充的像素数
    :return: 填充后的图像，numpy数组，shape为(height+2*pad, width+2*pad)
    """
    height, width = image.shape
    padded_image = np.zeros((height + 2 * pad, width + 2 * pad))
    padded_image[pad : height + pad, pad : width + pad] = image
    return padded_image


def juanji(img, kernel, x0, y0, kernel_size):
    value = 0
    for k in range(kernel_size):
        for t in range(kernel_size):
            ki = k - kernel_size // 2
            kj = t - kernel_size // 2
            img_value = int(img[kj + y0, ki + x0])
            value += img_value * kernel[t, k]
    return value


if __name__ == "__main__":
    guangban = cv.imread(
        "./src_fig/detection/center_light_field.bmp", cv.IMREAD_GRAYSCALE
    )
    h, w = guangban.shape  # -0.5-59.5
    # plt.imshow(guangban,cmap='gray')
    # plt.show()
    # gray=[]
    # #ax = plt.subplot(projection='3d')
    # for i in range(w):
    #     for j in range(h):
    #         #ax.bar3d(i,j,0,0.3,0.3,guangban[i,j],color='r')
    #         value = guangban[i,j]
    #         gray.append(value)
    # plt.hist(x=gray,bins=60)
    # plt.show()

    # #质心法
    # sum_gray = 0
    # sum_xgray = 0
    # sum_ygray = 0
    # sum_g2ray = 0
    # sum_xg2ray = 0
    # sum_yg2ray = 0
    # for i in range(w):
    #     for j in range(h):
    #         value = int(guangban[j,i])
    #         # sum_gray += value
    #         # sum_xgray += i * value
    #         # sum_ygray += j * value
    #         dvalue = value * value
    #         sum_g2ray += dvalue
    #         sum_xg2ray += i * dvalue
    #         sum_yg2ray += j * dvalue
    # # midx = sum_xgray / sum_gray
    # # midy = sum_ygray / sum_gray
    # midx = sum_xg2ray / sum_g2ray
    # midy = sum_yg2ray / sum_g2ray
    # print(f"width:{w}")
    # print(f"height:{h}")
    # print(f"x:{midx}")
    # print(f"y:{midy}")
    #
    # #H阵法
    # kernel_size = 5
    # C1 = np.zeros((h,w))    #  根据C值图求解x0,y0,c1是行列式，c2是对角线和
    # C2 = np.zeros((h, w))
    # guangban_pad = zero_pad(guangban,kernel_size // 2)      #  零填充w+4,h+4
    # kernelx = gauss_gx((kernel_size,kernel_size),1.5)     # 生成核
    # kernely = gauss_gy((kernel_size, kernel_size), 1.5)
    # kernelxx = gauss_gxx((kernel_size, kernel_size), 1.5)
    # kernelyy = gauss_gyy((kernel_size, kernel_size), 1.5)
    # kernelxy = gauss_gxy((kernel_size, kernel_size), 1.5)
    # for i in range(w):
    #     for j in range(h):
    #         x0 = i + kernel_size // 2
    #         y0 = j + kernel_size // 2
    #         gxx = juanji(guangban_pad,kernelxx,x0,y0,kernel_size)
    #         gxy = juanji(guangban_pad,kernelxy,x0,y0,kernel_size)
    #         gyy = juanji(guangban_pad, kernelyy, x0, y0, kernel_size)
    #         c1_value = gxx * gyy - gxy ** 2
    #         c2_value = gxx + gyy
    #         C1[j,i] = abs(c1_value)
    #         C2[j,i] = abs(c2_value)
    # # ax = plt.subplot(projection='3d')
    # # for i in range(h):
    # #     for j in range(w):
    # #         ax.bar3d(i,j,0,0.3,0.3,abs(C1[i,j]),color='r')
    # pos1 = np.argwhere(C1 == np.max(C1))
    # print(f"pos1:{pos1}")
    # pos2 = np.argwhere(C2 == np.max(C2))
    # print(f"pos2:{pos2}")
    # x0 = pos2[0,0]
    # y0 = pos2[0,1]
    # gx = juanji(guangban,kernelx,x0,y0,kernel_size)
    # gy = juanji(guangban,kernely,x0,y0,kernel_size)
    # gxx = juanji(guangban,kernelxx,x0,y0,kernel_size)
    # gxy = juanji(guangban,kernelxy,x0,y0,kernel_size)
    # gyy = juanji(guangban,kernelyy,x0,y0,kernel_size)
    # s = (gy*gxy-gx*gyy)/(gxx*gyy-gxy**2)
    # t = (gx*gxy - gy*gxx)/(gxx*gyy-gxy**2)
    # print(f"s,t:{s,t}")
    # print(f"xm,ym:{x0+s, y0+t}")
    # #plt.show()

    # # H阵法-使用cv卷积函数
    # kernel_size = 5
    # C1 = np.zeros((h, w))  # 根据C值图求解x0,y0,c1是行列式，c2是对角线和
    # C2 = np.zeros((h, w))
    # kernelx = gauss_gx((kernel_size, kernel_size), 1.5)  # 生成核
    # kernely = gauss_gy((kernel_size, kernel_size), 1.5)
    # kernelxx = gauss_gxx((kernel_size, kernel_size), 1.5)
    # kernelyy = gauss_gyy((kernel_size, kernel_size), 1.5)
    # kernelxy = gauss_gxy((kernel_size, kernel_size), 1.5)
    # kernelx_flip = cv.flip(kernelx, flipCode=-1)  # 将卷积核旋转180 度
    # kernely_flip = cv.flip(kernely, flipCode=-1)  # 将卷积核旋转180 度
    # kernelxx_flip = cv.flip(kernelxx, flipCode=-1)  # 将卷积核旋转180 度
    # kernelxy_flip = cv.flip(kernelxy, flipCode=-1)  # 将卷积核旋转180 度
    # kernelyy_flip = cv.flip(kernelyy, flipCode=-1)  # 将卷积核旋转180 度
    # gx = cv.filter2D(guangban, ddepth=cv.CV_64F, kernel=kernelx_flip, anchor=(-1, -1), borderType=cv.BORDER_CONSTANT)
    # gy = cv.filter2D(guangban, ddepth=cv.CV_64F, kernel=kernely_flip, anchor=(-1, -1), borderType=cv.BORDER_CONSTANT)
    # gxx = cv.filter2D(guangban, ddepth=cv.CV_64F, kernel=kernelxx_flip, anchor=(-1, -1), borderType=cv.BORDER_CONSTANT)
    # gxy = cv.filter2D(guangban, ddepth=cv.CV_64F, kernel=kernelxy_flip, anchor=(-1, -1), borderType=cv.BORDER_CONSTANT)
    # gyy = cv.filter2D(guangban, ddepth=cv.CV_64F, kernel=kernelyy_flip, anchor=(-1, -1), borderType=cv.BORDER_CONSTANT)
    # C1 = np.abs(gxx * gyy - gxy ** 2)
    # C2 = np.abs(gxx + gyy)
    # # for i in range(h):
    # #     for j in range(w):
    # #         gxx_val = float(gxx[i,j])
    # #         gxy_val = float(gxy[i, j])
    # #         gyy_val = float(gyy[i, j])
    # #         c1_val = np.abs(gxx_val * gyy_val - gxy_val ** 2)
    # #         c2_val = np.abs(gxx_val + gyy_val)
    # #         C1[i,j] = c1_val
    # #         C2[i,j] = c2_val
    # pos1 = np.argwhere(C1 == np.max(C1))
    # print(f"pos1:{pos1}")
    # pos2 = np.argwhere(C2 == np.max(C2))
    # print(f"pos2:{pos2}")
    # x0 = pos1[0, 0]
    # y0 = pos1[0, 1]
    # s = (gy[x0, y0] * gxy[x0, y0] - gx[x0, y0] * gyy[x0, y0]) / (gxx[x0, y0] * gyy[x0, y0] - gxy[x0, y0] ** 2)
    # t = (gx[x0, y0] * gxy[x0, y0] - gy[x0, y0] * gxx[x0, y0]) / (gxx[x0, y0] * gyy[x0, y0] - gxy[x0, y0] ** 2)
    # print(f"s,t:{s, t}")
    # print(f"xm,ym:{x0 + s, y0 + t}")

    # H阵法-使用scipy卷积
    kernel_size = 5
    C1 = np.zeros((h, w))  # 根据C值图求解x0,y0,c1是行列式，c2是对角线和
    C2 = np.zeros((h, w))
    kernelx = gauss_gx((kernel_size, kernel_size), 1.5)  # 生成核
    kernely = gauss_gy((kernel_size, kernel_size), 1.5)
    kernelxx = gauss_gxx((kernel_size, kernel_size), 1.5)
    kernelyy = gauss_gyy((kernel_size, kernel_size), 1.5)
    kernelxy = gauss_gxy((kernel_size, kernel_size), 1.5)
    # kernelx_flip = cv.flip(kernelx, flipCode=-1)  # 将卷积核旋转180 度
    # kernely_flip = cv.flip(kernely, flipCode=-1)  # 将卷积核旋转180 度
    # kernelxx_flip = cv.flip(kernelxx, flipCode=-1)  # 将卷积核旋转180 度
    # kernelxy_flip = cv.flip(kernelxy, flipCode=-1)  # 将卷积核旋转180 度
    # kernelyy_flip = cv.flip(kernelyy, flipCode=-1)  # 将卷积核旋转180 度
    gx = ss.convolve2d(guangban, kernelx, mode="same")
    gy = ss.convolve2d(guangban, kernely, mode="same")
    gxx = ss.convolve2d(guangban, kernelxx, mode="same")
    gxy = ss.convolve2d(guangban, kernelxy, mode="same")
    gyy = ss.convolve2d(guangban, kernelyy, mode="same")
    C1 = np.abs(gxx * gyy - gxy**2)
    C2 = np.abs(gxx + gyy)
    pos1 = np.argwhere(C1 == np.max(C1))
    print(f"pos1:{pos1}")
    pos2 = np.argwhere(C2 == np.max(C2))
    print(f"pos2:{pos2}")
    x0 = pos1[0, 0]
    y0 = pos1[0, 1]
    s = (gy[x0, y0] * gxy[x0, y0] - gx[x0, y0] * gyy[x0, y0]) / (
        gxx[x0, y0] * gyy[x0, y0] - gxy[x0, y0] ** 2
    )
    t = (gx[x0, y0] * gxy[x0, y0] - gy[x0, y0] * gxx[x0, y0]) / (
        gxx[x0, y0] * gyy[x0, y0] - gxy[x0, y0] ** 2
    )
    print(f"s,t:{s,t}")
    print(f"xm,ym:{x0+s, y0+t}")

    # #  高斯拟合法
    # B = np.zeros((5,5))     # 对称阵
    # C = np.zeros((5,1))
    # for i in range(w):
    #     for j in range(h):
    #         value = int(guangban[j,i])
    #         if(value != 0):
    #             B[0,0] += np.power(i,4)
    #             B[0,1] += i ** 2 * j ** 2
    #             B[0,2] += np.power(i,3)
    #             B[0, 3] += i ** 2 * j
    #             B[0, 4] += i ** 2
    #             B[1,1] += np.power(j,4)
    #             B[1,2] += i * j ** 2
    #             B[1,3] += np.power(j,3)
    #             B[1, 4] += j ** 2
    #             B[2,2] += i ** 2
    #             B[2,3] += i * j
    #             B[2,4] += i
    #             B[3,3] += j ** 2
    #             B[3,4] += j
    #             B[4,4] += 1
    #             C[0,0] += i ** 2 * np.log(value)
    #             C[1, 0] += j ** 2 * np.log(value)
    #             C[2, 0] += i * np.log(value)
    #             C[3, 0] += j * np.log(value)
    #             C[4, 0] += np.log(value)
    # for i in range(5):
    #     for j in range(5):
    #         if(j<i):
    #             B[i,j] = B[j,i]
    #         else:
    #             break
    # K = np.linalg.inv(np.array(B)) @ np.array(C)
    # x0 = -K[2,0] / (2* K[0,0])
    # y0 = -K[3,0] / (2* K[1,0])
    # print(f"x0,y0:{x0,y0}")
